#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "icar.h"
#include "icar_log.h"
#include "icar_pkg.h"

void
icar_pkg_print(uint8_t *src, size_t size)
{
	int i = 0;
	int m = 0;
	int len = size * 2 + 1;
	char *tmp = (char *)icar_malloc(len);
	memset(tmp, 0, len);

	for (i = 0; i < size; i++) {
		snprintf(tmp+m, len-m, "%02x", src[i] & 0xff);
		m += 2;
	}

	icar_daemon(LOG_DEBUG, "[%s]", tmp);

	free(tmp);
}

/*
   对有效数据进行校验，生成校验码，作为返回值
   src 不能为空，size要大于0
 */
uint8_t
icar_data_auth(uint8_t *src, size_t size)
{
	uint8_t code = 0;
	int i = 0;
	for ( ;i < size; i++) {
		code ^= src[i];
	}

	code ^= 0x7e;

	return code;
}

/* 
 对数据包添加消息头0x7e和消息尾0x7e并对其进行转义，并填入dest中，
  源数据为src，size参数为src的存储大小
 */
int
icar_pkg_trans_add_head_tail(uint8_t *dest, uint8_t *src, size_t size)
{
	if (!dest || !src || size == 0)
	{
		icar_daemon(LOG_ERR, "Invalid parameters");
		return -1;
	}

	int count = 0;
	int i = 0;

	dest[count++] = 0x7e;

	for ( ;i < size; i++) {
		if (src[i] == 0x7e) {
			dest[count++] = 0x7d;
			dest[count++] = 0x02;
		} else if (src[i] == 0x7d) {
			dest[count++] = 0x7d;
			dest[count++] = 0x01;
		} else
			dest[count++] = src[i];
	}

	dest[count++] = 0x7e;

	return count;
}

/* 
   对从终端接收到的数据包，去掉包头包尾的0x7e，反转义，校验。
   若成功，返回有效数据包的长度，
   若失败，返回负值
 */
int
icar_pkg_del_head_tail(uint8_t *dest, uint8_t *src, size_t size)
{
	if (!dest || !src || size == 0)
	{
		icar_daemon(LOG_ERR, "Invalid data format");
		return -1;
	}

	int count = 0;
	int i = 0;
	uint8_t code = 0;

	if ((src[0] != 0x7e) || src[size-1] != 0x7e)
	{
		icar_daemon(LOG_ERR, "Received eror message package");
		return -1;
	}

	for (i = 1; i < size-1; i++)
	{
		if (src[i] == 0x7d) {
			i++;
			if (src[i] == 0x02) {
				dest[count++] = 0x7e;
			} else if (src[i] == 0x01) {
				dest[count++] = 0x7d;
			} else
			{
				icar_daemon(LOG_ERR, "Received message has wrong format");
				return -1;
			}
		} else
			dest[count++] = src[i];
	}

	code = dest[count-1];


	if (code == icar_data_auth(dest, count-1)) {
		dest[count-1] = 0;
		count--;
	} else {
		icar_daemon(LOG_ERR, "Authenticate rises with an error");
		return -1;
	}

	return count;
}
